#include "StdAfx.h"
#include "Line.h"
#include <math.h>

using namespace cv;
using namespace std;

#define XTHRESH 50
#define YTHRESH 50

#define COMPARETHRESH 50
#define PI 3.14159265


Line::Line(Point p1, Point p2) : _p1(p1), _p2(p2)
{
	determineOrientation();
	this->intsecs = vector<Intersection>();
}

Line::Line(Point p1, Point p2, int lineId) : _p1(p1), _p2(p2), id(lineId)
{
	determineOrientation();
}

Line::Line()
{

}

void Line::addIntersection(Intersection intsec) {
	intsecs.push_back(intsec);
}

vector<Intersection> Line::getIntersectionPts() {
	return intsecs;
}

cv::Vec<int, 3> Line::getVec() {
	return Vec<int,3>(_p2.x - _p1.x, _p2.y - _p1.y, 0);
}

void Line::reFitLine() {

	if(intsecs.size() == 0)
		return;

	if(isVertical()) {
		reFitVerticalLine();
	}
	else if(isHorizontal()) {
		reFitHorizontalLine();
	}
}

//refit the vertical line with min y value intersection as start and max y intersection as end.
void Line::reFitVerticalLine() {
	int minIndx = 0;
	int maxIndx = 0;
	int minY = intsecs[0].intSecPt.y;
	int maxY = intsecs[0].intSecPt.y;

	for(size_t i = 1; i < intsecs.size(); i++) {
		int yVal = intsecs[i].intSecPt.y;

		if(yVal < minY) {
			minY = yVal;
			minIndx = i;
		}
		else if(yVal > maxY) {
			maxY = yVal;
			maxIndx = i;
		}
	}
	_p1 = intsecs[minIndx].intSecPt;
	_p2 = intsecs[maxIndx].intSecPt;
}

//refit the horizontal line with min x value intersection as start and max x intersection as end.
void Line::reFitHorizontalLine() {
	int minIndx = 0;
	int maxIndx = 0;
	int minX = intsecs[0].intSecPt.x;
	int maxX = intsecs[0].intSecPt.x;

	for(size_t i = 1; i < intsecs.size(); i++) {
		int xVal = intsecs[i].intSecPt.x;

		if(xVal < minX) {
			minX = xVal;
			minIndx = i;
		}
		else if(xVal > maxX) {
			maxX = xVal;
			maxIndx = i;
		}
	}
	_p1 = intsecs[minIndx].intSecPt;
	_p2 = intsecs[maxIndx].intSecPt;
}

vector<Point> Line::getPoints()
{
	vector<Point> points;
	points.push_back(_p1);
	points.push_back(_p2);
	return points;
}

//creates direction vector for the line.
void Line::createDirVec() {
	dirVec = Mat(1, 3, CV_64F);
	int xdiff = _p2.x - _p1.x;
	int ydiff = _p2.y - _p1.y;
	double length = sqrt(pow(_p1.x - _p2.x, 2.0) + pow(_p1.y - _p2.y, 2.0));

	dirVec.at<double>(0,0) = xdiff / length;
	dirVec.at<double>(0,1) = ydiff / length;
	dirVec.at<double>(0,2) = 1;
}


//extend small lines to inifinite lines/lines reaches out of frame.
Line Line::extendLine(int yBound, int xBound) {

	int vx1 = _p1.x - _p2.x;
	int vy1 = _p1.y - _p2.y;

	int newP1X = _p1.x;
	int newP1Y = _p1.y;

	double dist = 0;

	while(newP1X > 0 && newP1X < xBound &&
		newP1Y > 0 && newP1Y < yBound) {
		newP1X += vx1;
		newP1Y += vy1;
	}

	int newP2X = _p2.x;
	int newP2Y = _p2.y;

	while(newP2X > 0 && newP2X < xBound &&
		newP2Y > 0 && newP2Y < yBound) {
		newP2X -= vx1;
		newP2Y -= vy1;
	}
	
	_p1 = Point(newP1X, newP1Y);
	_p2 = Point(newP2X, newP2Y);
	
	Line l(_p1, _p2);
	return l;
}

Point Line::findIntersection(Line bLine) {

	Point p1 = _p1;
	Point p2 = _p2;
	vector<Point> points = bLine.getPoints();
	Point p3 = points[0];
	Point p4 = points[1];
	

	//NOT USED. Might contain a bug.
	/*int xCordNum = (p1.x*p2.y - p1.y*p2.x)*(p3.x - p4.x) - 
		(p1.x - p2.x)*(p3.x*p4.y - p3.y*p4.x);


	int xCordDenom = (p1.x - p2.x)*(p3.y - p4.y) -
		(p1.y - p2.y)*(p3.x - p4.x);	
	int yCordNum = (p1.x*p2.y - p1.y*p2.x)*(p3.y - p4.y) -
		(p1.y - p2.y)*(p3.x*p4.y - p3.x*p4.y - p3.y*p4.x);
	int yCordDenom = (p1.x - p2.x)*(p3.y - p4.y) -
		(p1.y - p2.y)*(p3.x - p4.x);
	
	if(xCordDenom == 0 || yCordDenom == 0) {
		return Point(-1,-1);
	}
	int newX = cvRound(xCordNum / xCordDenom);
	int newY = cvRound(yCordNum / yCordDenom);*/


	double A1 = p2.y - p1.y;
	double B1 = p1.x - p2.x;
	double C1 = B1*p1.y + A1 * p1.x;

	double A2 = p4.y - p3.y;
	double B2 = p3.x - p4.x;
	double C2 = B2*p3.y + A2 * p3.x; 

	double det = A1 *B2 - A2 *B1;
	if(det == 0) {
		return Point(-1,-1);
	}

	int newX = (int)((B2*C1-B1*C2)/det);
	int newY = (int)((A1*C2-A2*C1)/det);

	Point p((int)newX, (int)newY);
	return p;
}

//draw line on a given Mat with the given color.
void Line::drawLine(Mat m, Scalar color) {
	cv::line(m, _p1, _p2, color, 3, CV_AA);
	circle(m, _p2, 2, Scalar(0,255,0));
	circle(m, _p1, 2, Scalar(0,255,0));
}

//draw line on a given Mat with default color.
void Line::drawLine(Mat m)
{
	cv::line(m, _p1, _p2, Scalar(0,0,255), 3, CV_AA);
	circle(m, _p2, 2, Scalar(0,255,0));
	circle(m, _p1, 2, Scalar(0,255,0));
}

void Line::printLine() {
	cout << "(" << _p1.x << " , " << _p1.y << ")" << " + ";
	cout << "(" << _p2.x << " , " << _p2.y << ")" << endl;
}

double Line::getLength(){
	return sqrt(pow( (double)abs(_p1.x-_p2.x),2) + pow( (double)abs(_p1.y-_p2.y),2));
}

//compute the angle of the line.(between 0 and 360)
double Line::getAngle() {
	double vx = _p2.x - _p1.x;
	double vy = _p2.y - _p1.y;

	double angle = atan(vy / vx) * 180 / PI;
	if(angle < 0){
		angle +=360;
	}
	return angle;
}

//determines if the line is vertical or horizontal.
char Line::determineOrientation()
{
	double xDiff = abs(_p1.x - _p2.x);
	double yDiff = abs(_p1.y - _p2.y);
	//account for perspective distortion.
	xDiff = xDiff*0.5;

	if(xDiff < yDiff) {
		_orientation = 1;
	}
	else if(xDiff > yDiff) {
		_orientation = 2;
	}
	else {
		_orientation = 0;
	}
	return _orientation;
}

char Line::getOrientation()
{
	return _orientation;
}

bool Line::isHorizontal()
{
	return _orientation == 2;
}

bool Line::isVertical()
{
	return _orientation == 1;
}

Line::~Line(void)
{
}
